/*   The MIT License
 *   
 *   Carina Engine
 *   Copyright (c) 2009 2010 2011 2012 Zdravko Velinov
 *   
 *   Permission is hereby granted, free of charge, to any person obtaining a copy
 *   of this software and associated documentation files (the "Software"), to deal
 *   in the Software without restriction, including without limitation the rights
 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *   copies of the Software, and to permit persons to whom the Software is
 *   furnished to do so, subject to the following conditions:
 *
 *   The above copyright notice and this permission notice shall be included in
 *   all copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 *   THE SOFTWARE.
 */

#ifndef MATH_HH_
#define MATH_HH_

#include <cassert>
#include <cmath>
#include <cfloat>

#include <iostream>

#include <xmmintrin.h>

#include "carina/common/global.hh"
#include "carina/common/exception.hh"

#ifdef _MSC_VER
#define COPYSIGN(x, y) (float)_copysign(x, y)
#elif defined(LINUX) || defined(__MINGW32__)
#define COPYSIGN(x, y) copysign(x, y)
#else
#	error "Unsupported platform"
#endif

namespace Carina
{
/*! \defgroup CarinaMath Math
    @{
*/
#ifdef _MSC_VER
unsigned log2(unsigned x);
#endif

//! Mathematical constant
const float math_pi = 3.141592f;

//! Returns true if two floating-point numbers are almost equal or equal
#define CMPF(a, b) (fabs(a - b) < FLT_EPSILON*1e1f)
#define NCMPF(a, b) (fabs(a - b) >= FLT_EPSILON*1e1f)

//! Returns the closest power of two number
/*!
    \param num an integer argument
    \return closest power of two number to num
 */
unsigned pow_of_2(unsigned num);

//! Checks if parameter num is a number of the form 2^n+1
/*!
    \param num an integer argument
    \return true if num is power of two plus one
*/
bool is_not_pow_2_1(unsigned num);

//! Checks if parameter num is a number of the form 2^n
/*!
    \param num an integer argument
    \return true if num is power of two
*/
bool is_not_pow_2(unsigned num);

//! Computes the closest integer value
/*!
    \param val a floating-point number argument
    \return the closest integer value as a floating-point variable
*/
inline float round(float val) { return floor(val + 0.5f); }

//! Converts degrees to radians
/*!
    \param val a floating-point number argument
    \return the angle in radians
*/
inline float to_radians(float val) { return (val * math_pi) / 180.0f; }

inline float to_degrees(float val) { return (val * 180.0f) / math_pi; }

//! Clamps a floating-point variable to range
float clampf(float val, float min, float max);

//! Clamps an integer variable to range
int clampi(int val, int min, int max);
/*! @} */


struct Vector3;
struct Vector4;
struct Quaternion;
struct DualQuaternion;

//! 2-dimensional vector
/*! \ingroup CarinaMath
*/
struct Vector2
{
    struct coord
    {
        float x, /*!< x-coordinate component */
              y; /*!< y-coordinate component */
    };

    union
    {
        coord coordinate;
        float elem[2]; /*!< coordinate array */
    };

    //! Default constructor
    explicit Vector2() { STATIC_ASSERT(sizeof(Vector2) == 2*sizeof(float), "Vector2 has the wrong size"); }

    //! Constructor
    /*!
        \param _x x-coordinate component
        \param _y y-coordinate component
    */
    Vector2(float _x, float _y)
    {
        set(_x, _y);
    }

    Vector2(const Vector2& v);
    
    //! Conversion constructor
    /*! It copies only the x- and y-component
        \param v a 3-dimensional vector
     */
    explicit Vector2(const Vector3& v);

    //! Conversion constructor
    /*! It copies only the x- and y-component
        \param v a 4-dimensional vector
     */
    explicit Vector2(const Vector4& v);

    inline float& x() { return coordinate.x; }
    inline float& y() { return coordinate.y; }
    inline float& r() { return coordinate.x; }
    inline float& g() { return coordinate.y; }
    inline float& s() { return coordinate.x; }
    inline float& t() { return coordinate.y; }

    inline float x() const { return coordinate.x; }
    inline float y() const { return coordinate.y; }
    inline float r() const { return coordinate.x; }
    inline float g() const { return coordinate.y; }
    inline float s() const { return coordinate.x; }
    inline float t() const { return coordinate.y; }

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float& operator[](size_t i)
    {
#ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
#endif
        return elem[i];
    }

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float operator[](size_t i) const
    {
    #ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return elem[i];
    }
    //! Computes the length of the vector
    /*!
        \return the length of the vector as a floating-point number
    */
    float length() const { return sqrt(coordinate.x*coordinate.x+coordinate.y*coordinate.y); }
    
    //! Normalizes the vector
    /*!
        \remarks the function doesn't do anything if length is zero
    */
    void normalize()
    {
        float l = length();
        if(l != 0.0f)
        {
            coordinate.x /= l;
            coordinate.y /= l;
        }
    }

    //! Computes the dot product between two vectors
    /*!
        \param vec a 2-dimesional vector
        \return the dot product between the vectors
    */
    float dot(const Vector2& vec) const { return coordinate.x*vec.coordinate.x + coordinate.y*vec.coordinate.y; }

    //! Sets the values of the coordinate component
    /*!
        \param _x x-coordinate component
        \param _y y-coordinate component
    */
    inline void set(float _x, float _y) { coordinate.x = _x; coordinate.y = _y; }
};

inline bool operator==(const Vector2& lhs, const Vector2& rhs) { return CMPF(lhs.coordinate.x, rhs.coordinate.x) && CMPF(lhs.coordinate.y, rhs.coordinate.y); }

inline bool operator!=(const Vector2& lhs, const Vector2& rhs) { return NCMPF(lhs.coordinate.x, rhs.coordinate.x) || NCMPF(lhs.coordinate.y, rhs.coordinate.y); }

//! Sums two vectors and returns the resulting vector
/*! \related Vector2 */
inline Vector2 operator+(const Vector2& lhs, const Vector2& rhs) { return Vector2(lhs.coordinate.x+rhs.coordinate.x, lhs.coordinate.y+rhs.coordinate.y); }

//! Subtracts two vectors and returns the resulting vector
/*! \related Vector2 */
inline Vector2 operator-(const Vector2& lhs, const Vector2& rhs) { return Vector2(lhs.coordinate.x-rhs.coordinate.x, lhs.coordinate.y-rhs.coordinate.y); }

//! Negates a vector and returns the resulting vector
/*! \related Vector2 */
inline Vector2 operator-(const Vector2& vec) { return Vector2(-vec.coordinate.x, -vec.coordinate.y); }

//! Sums two vectors and stores the result in the first vector
/*! \related Vector2 */
inline Vector2& operator+=(Vector2& lhs, const Vector2& rhs) { lhs.coordinate.x += rhs.coordinate.x; lhs.coordinate.y += rhs.coordinate.y; return lhs; }

//! Subtracts two vectors and stores the result in the first vector
/*! \related Vector2 */
inline Vector2& operator-=(Vector2& lhs, const Vector2& rhs) { lhs.coordinate.x -= rhs.coordinate.x; lhs.coordinate.y -= rhs.coordinate.y; return lhs; }

//! Multiplies a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector2 */
inline Vector2 operator*(const Vector2& vec, float a) { return Vector2(vec.coordinate.x * a, vec.coordinate.y * a); }

//! Multiplies a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector2 */
inline Vector2 operator*(float a, const Vector2& vec) { return Vector2(vec.coordinate.x * a, vec.coordinate.y * a); }

//! Multiplies a vector with a float-pointing variable and replaces the vector
/*! \related Vector2 */
inline Vector2& operator*=(Vector2& vec, float a) { vec.coordinate.x *= a; vec.coordinate.y *= a; return vec; }

//! Divides a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector2 */
inline Vector2 operator/(const Vector2& vec, float a) { return Vector2(vec.coordinate.x / a, vec.coordinate.y / a); }

//! Divides a vector with a float-pointing variable and replaces the vector
/*! \related Vector2 */
inline Vector2& operator/=(Vector2& vec, float a) { vec.coordinate.x /= a; vec.coordinate.y /= a; return vec; }

struct Vector4;

//! 3-dimensional vector
/*! \ingroup CarinaMath
*/
struct Vector3
{
    struct coord
    {
        float x, /*!< x-coordinate component */
              y, /*!< y-coordinate component */
              z; /*!< z-coordinate component */
    };

    struct comb0
    {
        Vector2 xy; /*!< xy-component subvector */
        float   z;  /*!< z-coordinate component */
    };

    struct comb1
    {
        float   x;  /*!< x-coordinate component */
        Vector2 yz; /*!< yz-component subvector */
    };

    union
    {
        coord coordinate;
#ifdef _CXX11_SUPPORT
        comb0 combined0;
        comb1 combined1;
#endif
        float elem[3]; /*!< coordinate array */
    };

    //! Default constructor
    Vector3() { STATIC_ASSERT(sizeof(Vector3) == 3*sizeof(float), "Vector3 has the wrong size"); }

    Vector3(const Vector3& v);
    
    //! Conversion constructor
    explicit Vector3(const Vector4& v);

    //! Constructor
    /*!
        \param _x x-coordinate component
        \param _y y-coordinate component
        \param _z z-coordinate component
    */
    Vector3(float _x, float _y, float _z)
    {
        set(_x, _y, _z);
    }

    inline float& x() { return coordinate.x; }
    inline float& y() { return coordinate.y; }
    inline float& z() { return coordinate.z; }
    inline float& r() { return coordinate.x; }
    inline float& g() { return coordinate.y; }
    inline float& b() { return coordinate.z; }
    inline float& s() { return coordinate.x; }
    inline float& t() { return coordinate.y; }
    inline float& p() { return coordinate.z; }
#ifdef _CXX11_SUPPORT
    inline Vector2& xy() { return combined0.xy; }
    inline Vector2& yz() { return combined1.yz; }
    inline Vector2& rg() { return combined0.xy; }
    inline Vector2& gb() { return combined1.yz; }
    inline Vector2& st() { return combined0.xy; }
    inline Vector2& tp() { return combined1.yz; }
#else
    inline Vector2& xy() { return reinterpret_cast<comb0*>(this)->xy; }
    inline Vector2& yz() { return reinterpret_cast<comb1*>(this)->yz; }
    inline Vector2& rg() { return reinterpret_cast<comb0*>(this)->xy; }
    inline Vector2& gb() { return reinterpret_cast<comb1*>(this)->yz; }
    inline Vector2& st() { return reinterpret_cast<comb0*>(this)->xy; }
    inline Vector2& tp() { return reinterpret_cast<comb1*>(this)->yz; }
#endif

    inline float x() const { return coordinate.x; }
    inline float y() const { return coordinate.y; }
    inline float z() const { return coordinate.z; }
    inline float r() const { return coordinate.x; }
    inline float g() const { return coordinate.y; }
    inline float b() const { return coordinate.z; }
    inline float s() const { return coordinate.x; }
    inline float t() const { return coordinate.y; }
    inline float p() const { return coordinate.z; }
#ifdef _CXX11_SUPPORT
    inline Vector2 xy() const { return combined0.xy; }
    inline Vector2 yz() const { return combined1.yz; }
    inline Vector2 rg() const { return combined0.xy; }
    inline Vector2 gb() const { return combined1.yz; }
    inline Vector2 st() const { return combined0.xy; }
    inline Vector2 tp() const { return combined1.yz; }
#else
    inline Vector2 xy() const { return reinterpret_cast<const comb0*>(this)->xy; }
    inline Vector2 yz() const { return reinterpret_cast<const comb1*>(this)->yz; }
    inline Vector2 rg() const { return reinterpret_cast<const comb0*>(this)->xy; }
    inline Vector2 gb() const { return reinterpret_cast<const comb1*>(this)->yz; }
    inline Vector2 st() const { return reinterpret_cast<const comb0*>(this)->xy; }
    inline Vector2 tp() const { return reinterpret_cast<const comb1*>(this)->yz; }
#endif

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float& operator[](size_t i)
    {
    #ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return elem[i];
    }

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float operator[](size_t i) const
    {
    #ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return elem[i];
    }

    //! Computes the length of the vector
    /*!
        \return the length of the vector as a floating-point number
    */
    float length() const { return sqrt(coordinate.x*coordinate.x+coordinate.y*coordinate.y+coordinate.z*coordinate.z); }

    //! Normalizes the vector
    /*!
        \remarks the function doesn't do anything if length is zero
    */
    void normalize()
    {
        float l = length();
        if(l != 0.0f)
        {
            coordinate.x /= l;
            coordinate.y /= l;
            coordinate.z /= l;
        }
    }

    //! Computes the dot product between two vectors
    /*! 
        \param vec a 3-dimesional vector
        \return the dot product between the vectors
    */
    float dot(const Vector3& vec) const { return coordinate.x*vec.coordinate.x + coordinate.y*vec.coordinate.y + coordinate.z*vec.coordinate.z; }

    //! Computes the dot product between two vectors
    /*! \remarks this function assumes that the w component for a 3-dimensional vector is equal to 1.0
        \param vec a 4-dimesional vector
        \return the dot product between the vectors
    */
    float dot(const Vector4& vec) const;

    //! Computes the cross product between two vectors
    /*!
        \param vec a 3-dimensional vector
        \return the cross product between the vectors
    */
    Vector3 cross(const Vector3& vec) const { return Vector3(coordinate.y*vec.coordinate.z - coordinate.z*vec.coordinate.y,
                                                             coordinate.z*vec.coordinate.x - coordinate.x*vec.coordinate.z,
                                                             coordinate.x*vec.coordinate.y - coordinate.y*vec.coordinate.x); }

    //! Sets the values of the coordinate component
    /*!
        \param _x x-coordinate component
        \param _y y-coordinate component
        \param _z z-coordinate component
    */
    inline void set(float _x, float _y, float _z) { coordinate.x = _x; coordinate.y = _y; coordinate.z = _z; }
};

inline bool operator==(const Vector3& lhs, const Vector3& rhs) { return CMPF(lhs.coordinate.x, rhs.coordinate.x) && CMPF(lhs.coordinate.y, rhs.coordinate.y) && CMPF(lhs.coordinate.z, rhs.coordinate.z); }

inline bool operator!=(const Vector3& lhs, const Vector3& rhs) { return NCMPF(lhs.coordinate.x, rhs.coordinate.x) || CMPF(lhs.coordinate.y, rhs.coordinate.y) || NCMPF(lhs.coordinate.z, rhs.coordinate.z); }

//! Sums two vectors and returns the resulting vector
/*! \related Vector3 */
inline Vector3 operator+(const Vector3& lhs, const Vector3& rhs) { return Vector3(lhs.coordinate.x+rhs.coordinate.x, lhs.coordinate.y+rhs.coordinate.y, lhs.coordinate.z+rhs.coordinate.z); }

//! Subtracts two vectors and returns the resulting vector
/*! \related Vector3 */
inline Vector3 operator-(const Vector3& lhs, const Vector3& rhs) { return Vector3(lhs.coordinate.x-rhs.coordinate.x, lhs.coordinate.y-rhs.coordinate.y, lhs.coordinate.z-rhs.coordinate.z); }

//! Negates a vector and returns the resulting vector
/*! \related Vector3 */
inline Vector3 operator-(const Vector3& vec) { return Vector3(-vec.coordinate.x, -vec.coordinate.y, -vec.coordinate.z); }

//! Sums two vectors and stores the result in the first vector
/*! \related Vector3 */
inline Vector3& operator+=(Vector3& lhs, const Vector3& rhs) { lhs.coordinate.x += rhs.coordinate.x; lhs.coordinate.y += rhs.coordinate.y; lhs.coordinate.z += rhs.coordinate.z; return lhs; }

//! Subtracts two vectors and stores the result in the first vector
/*! \related Vector3 */
inline Vector3& operator-=(Vector3& lhs, const Vector3& rhs) { lhs.coordinate.x -= rhs.coordinate.x; lhs.coordinate.y -= rhs.coordinate.y; lhs.coordinate.z -= rhs.coordinate.z; return lhs; }

//! Multiplies a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector3 */
inline Vector3 operator*(const Vector3& vec, float a) { return Vector3(vec.coordinate.x * a, vec.coordinate.y * a, vec.coordinate.z * a); }

//! Multiplies a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector3 */
inline Vector3 operator*(float a, const Vector3& vec) { return Vector3(vec.coordinate.x * a, vec.coordinate.y * a, vec.coordinate.z * a); }

//! Multiplies a vector with a float-pointing variable and replaces the vector
/*! \related Vector3 */
inline Vector3& operator*=(Vector3& vec, float a) { vec.coordinate.x *= a; vec.coordinate.y *= a; vec.coordinate.z *= a; return vec; }

//! Divides a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector3 */
inline Vector3 operator/(const Vector3& vec, float a) { return Vector3(vec.coordinate.x / a, vec.coordinate.y / a, vec.coordinate.z / a); }

//! Divides a vector with a float-pointing variable and replaces the vector
/*! \related Vector3 */
inline Vector3& operator/=(Vector3& vec, float a) { vec.coordinate.x /= a; vec.coordinate.y /= a; vec.coordinate.z /= a; return vec; }

//! Returns the component-wise absolute value of a 3-dimensional vector
inline Vector3 v3abs(Vector3& v)
{
    return Vector3(fabs(v.coordinate.x), fabs(v.coordinate.y), fabs(v.coordinate.z));
}

inline Vector3 to_degrees(const Vector3& vec) { return Vector3(to_degrees(vec.coordinate.x), to_degrees(vec.coordinate.y), to_degrees(vec.coordinate.z)); }

inline Vector3 to_radians(const Vector3& vec) { return Vector3(to_radians(vec.coordinate.x), to_radians(vec.coordinate.y), to_radians(vec.coordinate.z)); }

//! 4-dimensional vector
/*!
    \ingroup CarinaMath
*/
struct Vector4
{
    struct coord
    {
        float x, /*!< x-coordinate component */
              y, /*!< y-coordinate component */
              z, /*!< z-coordinate component */
              w; /*!< w-coordinate component */
    };
    struct comb0
    {
        Vector2 xy, /*!< xy-component subvector */
                zw; /*!< zw-component subvector */
    };
    struct comb1
    {
        float   x;  /*!< x-coordinate component */
        Vector2 yz; /*!< yz-component subvector */
        float   w;  /*!< w-coordinate component */
    };
    struct comb2
    {
        Vector3 xyz; /*!< xyz-component subvector */
        float   w;   /*!< w-coordinate component subvector */
    };
    struct comb3
    {
        float   x;   /*!< x-coordinate component */
        Vector3 yzw; /*!< yzw-component subvector */
    };

    union
    {
        coord coordinate;
#ifdef _CXX11_SUPPORT
        comb0 combined0;
        comb1 combined1;
        comb2 combined2;
        comb3 combined3;
#endif
        float elem[4]; /*!< coordinate array */
    };

    //! Default constructor
    Vector4() { STATIC_ASSERT(sizeof(Vector4) == 4*sizeof(float), "Vector4 has the wrong size"); }

    Vector4(const Vector4& vec);
    
    //! Conversion constructor
    explicit Vector4(const Vector3& vec)
    {
        set(vec.coordinate.x, vec.coordinate.y, vec.coordinate.z, 1.0f);
    }

    //! Constructor
    /*!
        \param _x x-coordinate component
        \param _y y-coordinate component
        \param _z z-coordinate component
        \param _w w-coordinate component
    */
    Vector4(float _x, float _y, float _z, float _w)
    {
        set(_x, _y, _z, _w);
    }

    inline float& x() { return coordinate.x; }
    inline float& y() { return coordinate.y; }
    inline float& z() { return coordinate.z; }
    inline float& w() { return coordinate.w; }
    inline float& r() { return coordinate.x; }
    inline float& g() { return coordinate.y; }
    inline float& b() { return coordinate.z; }
    inline float& a() { return coordinate.w; }
    inline float& s() { return coordinate.x; }
    inline float& t() { return coordinate.y; }
    inline float& p() { return coordinate.z; }
    inline float& q() { return coordinate.w; }

#ifdef _CXX11_SUPPORT
    inline Vector2& xy() { return combined0.xy; }
    inline Vector2& yz() { return combined1.yz; }
    inline Vector2& zw() { return combined0.zw; }
    inline Vector3& xyz() { return combined2.xyz; }
    inline Vector3& yzw() { return combined3.yzw; }
    inline Vector2& rg() { return combined0.xy; }
    inline Vector2& gb() { return combined1.yz; }
    inline Vector2& ba() { return combined0.zw; }
    inline Vector3& rgb() { return combined2.xyz; }
    inline Vector3& gba() { return combined3.yzw; }
    inline Vector2& st() { return combined0.xy; }
    inline Vector2& tp() { return combined1.yz; }
    inline Vector2& pq() { return combined0.zw; }
    inline Vector3& stp() { return combined2.xyz; }
    inline Vector3& tpq() { return combined3.yzw; }
#else
    inline Vector2& xy() { return reinterpret_cast<comb0*>(this)->xy; }
    inline Vector2& yz() { return reinterpret_cast<comb1*>(this)->yz; }
    inline Vector2& zw() { return reinterpret_cast<comb0*>(this)->zw; }
    inline Vector3& xyz() { return reinterpret_cast<comb2*>(this)->xyz; }
    inline Vector3& yzw() { return reinterpret_cast<comb3*>(this)->yzw; }
    inline Vector2& rg() { return reinterpret_cast<comb0*>(this)->xy; }
    inline Vector2& gb() { return reinterpret_cast<comb1*>(this)->yz; }
    inline Vector2& ba() { return reinterpret_cast<comb0*>(this)->zw; }
    inline Vector3& rgb() { return reinterpret_cast<comb2*>(this)->xyz; }
    inline Vector3& gba() { return reinterpret_cast<comb3*>(this)->yzw; }
    inline Vector2& st() { return reinterpret_cast<comb0*>(this)->xy; }
    inline Vector2& tp() { return reinterpret_cast<comb1*>(this)->yz; }
    inline Vector2& pq() { return reinterpret_cast<comb0*>(this)->zw; }
    inline Vector3& stp() { return reinterpret_cast<comb2*>(this)->xyz; }
    inline Vector3& tpq() { return reinterpret_cast<comb3*>(this)->yzw; }
#endif

    inline float x() const { return coordinate.x; }
    inline float y() const { return coordinate.y; }
    inline float z() const { return coordinate.z; }
    inline float w() const { return coordinate.w; }
    inline float r() const { return coordinate.x; }
    inline float g() const { return coordinate.y; }
    inline float b() const { return coordinate.z; }
    inline float a() const { return coordinate.w; }
    inline float s() const { return coordinate.x; }
    inline float t() const { return coordinate.y; }
    inline float p() const { return coordinate.z; }
    inline float q() const { return coordinate.w; }

#ifdef _CXX11_SUPPORT
    inline Vector2 xy() const { return combined0.xy; }
    inline Vector2 yz() const { return combined1.yz; }
    inline Vector2 zw() const { return combined0.zw; }
    inline Vector3 xyz() const { return combined2.xyz; }
    inline Vector3 yzw() const { return combined3.yzw; }
    inline Vector2 rg() const { return combined0.xy; }
    inline Vector2 gb() const { return combined1.yz; }
    inline Vector2 ba() const { return combined0.zw; }
    inline Vector3 rgb() const { return combined2.xyz; }
    inline Vector3 gba() const { return combined3.yzw; }
    inline Vector2 st() const { return combined0.xy; }
    inline Vector2 tp() const { return combined1.yz; }
    inline Vector2 pq() const { return combined0.zw; }
    inline Vector3 stp() const { return combined2.xyz; }
    inline Vector3 tpq() const { return combined3.yzw; }
#else
    inline Vector2 xy() const { return reinterpret_cast<const comb0*>(this)->xy; }
    inline Vector2 yz() const { return reinterpret_cast<const comb1*>(this)->yz; }
    inline Vector2 zw() const { return reinterpret_cast<const comb0*>(this)->zw; }
    inline Vector3 xyz() const { return reinterpret_cast<const comb2*>(this)->xyz; }
    inline Vector3 yzw() const { return reinterpret_cast<const comb3*>(this)->yzw; }
    inline Vector2 rg() const { return reinterpret_cast<const comb0*>(this)->xy; }
    inline Vector2 gb() const { return reinterpret_cast<const comb1*>(this)->yz; }
    inline Vector2 ba() const { return reinterpret_cast<const comb0*>(this)->zw; }
    inline Vector3 rgb() const { return reinterpret_cast<const comb2*>(this)->xyz; }
    inline Vector3 gba() const { return reinterpret_cast<const comb3*>(this)->yzw; }
    inline Vector2 st() const { return reinterpret_cast<const comb0*>(this)->xy; }
    inline Vector2 tp() const { return reinterpret_cast<const comb1*>(this)->yz; }
    inline Vector2 pq() const { return reinterpret_cast<const comb0*>(this)->zw; }
    inline Vector3 stp() const { return reinterpret_cast<const comb2*>(this)->xyz; }
    inline Vector3 tpq() const { return reinterpret_cast<const comb3*>(this)->yzw; }
#endif

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float& operator[](size_t i) 
    {
    #ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return elem[i];
    }

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float operator[](size_t i) const
    {
    #ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return elem[i];
    }

    //! Computes the length of the vector
    /*! This function uses the following equation:
        \f[\|\vec{v}\| = \frac{\sqrt{v_{x}^{2} + v_{y}^{2} + v_{z}^{2}}}{v_w}\f]
        \return the length of the vector as a floating-point number
        \warning the function doesn't check for \f$v_w \neq 0\f$
    */
    float length() const { return sqrt(coordinate.x*coordinate.x+coordinate.y*coordinate.y+coordinate.z*coordinate.z)/coordinate.w; }

    //! Partially normalizes the component of the vector
    /*! The function divides all of the coordinate component by:
        \f[\|\vec{v_{xyz}}\| = \sqrt{v_{x}^{2} + v_{y}^{2} + v_{z}^{2}}\f]
        \warning \f$\vec{v_w} = \frac{\vec{v_w}}{|\vec{v_{xyz}}\|}\f$
    */
    void normalizePartial()
    {
        float l = sqrt(coordinate.x*coordinate.x+coordinate.y*coordinate.y+coordinate.z*coordinate.z);
        if(l != 0.0f)
        {
            coordinate.x /= l;
            coordinate.y /= l;
            coordinate.z /= l;
            coordinate.w /= l;
        }
    }

    //! Computes the dot product between two vectors
    /*! The formula used for this function is as follows: 
        \f[a_x b_x + a_y b_y + a_z b_z + a_w b_w\f]
        \param vec a 4-dimensional vector
        \returns the dot product between the two vectors
    */
    float dot(const Vector4& vec) const { return coordinate.x*vec.coordinate.x + coordinate.y*vec.coordinate.y + coordinate.z*vec.coordinate.z + coordinate.w*vec.coordinate.w; }

    //! Computes the dot product between two vectors
    /*! The formula used for this function is as follows: 
        \f[a_x b_x + a_y b_y + a_z b_z + a_w\f]
        Where \f$\vec{a}\f$ is this vector.
        \param vec a 3-dimensional vector
        \returns the dot product between the two vectors
    */
    float dot(const Vector3& vec) const { return coordinate.x*vec.coordinate.x + coordinate.y*vec.coordinate.y + coordinate.z*vec.coordinate.z + coordinate.w; }

    //! Sets the values of the coordinate component
    /*!
        \param _x x-coordinate component
        \param _y y-coordinate component
        \param _z z-coordinate component
        \param _w w-coordinate component
    */
    inline void set(float _x, float _y, float _z, float _w = 1.0f) { coordinate.x = _x; coordinate.y = _y; coordinate.z = _z; coordinate.w = _w; }
};

inline bool operator==(const Vector4& lhs, const Vector4& rhs) { return CMPF(lhs.coordinate.x, rhs.coordinate.x) && CMPF(lhs.coordinate.y, rhs.coordinate.y) && CMPF(lhs.coordinate.z, rhs.coordinate.z) && CMPF(lhs.coordinate.w, rhs.coordinate.w); }

inline bool operator!=(const Vector4& lhs, const Vector4& rhs) { return NCMPF(lhs.coordinate.x, rhs.coordinate.x) || NCMPF(lhs.coordinate.y, rhs.coordinate.y) || NCMPF(lhs.coordinate.z, rhs.coordinate.z) || NCMPF(lhs.coordinate.w, rhs.coordinate.w); }

//! Sums two vectors and returns the resulting vector
/*! \related Vector4 */
inline Vector4 operator+(const Vector4& lhs, const Vector4& rhs) { return Vector4(lhs.coordinate.x+rhs.coordinate.x, lhs.coordinate.y+rhs.coordinate.y, lhs.coordinate.z+rhs.coordinate.z, lhs.coordinate.w+rhs.coordinate.w); }

//! Subtracts two vectors and returns the resulting vector
/*! \related Vector4 */
inline Vector4 operator-(const Vector4& lhs, const Vector4& rhs) { return Vector4(lhs.coordinate.x-rhs.coordinate.x, lhs.coordinate.y-rhs.coordinate.y, lhs.coordinate.z-rhs.coordinate.z, lhs.coordinate.w-rhs.coordinate.w); }

//! Negates a vector and returns the resulting vector
/*! \related Vector4 */
inline Vector4 operator-(const Vector4& vec) { return Vector4(-vec.coordinate.x, -vec.coordinate.y, -vec.coordinate.z, -vec.coordinate.w); }

//! Sums two vectors and stores the result in the first vector
/*! \related Vector4 */
inline Vector4& operator+=(Vector4& lhs, const Vector4& rhs) { lhs.coordinate.x += rhs.coordinate.x; lhs.coordinate.y += rhs.coordinate.y; lhs.coordinate.z += rhs.coordinate.z; lhs.coordinate.w += rhs.coordinate.w; return lhs; }

//! Subtracts two vectors and stores the result in the first vector
/*! \related Vector4 */
inline Vector4& operator-=(Vector4& lhs, const Vector4& rhs) { lhs.coordinate.x -= rhs.coordinate.x; lhs.coordinate.y -= rhs.coordinate.y; lhs.coordinate.z -= rhs.coordinate.z; lhs.coordinate.w -= rhs.coordinate.w; return lhs; }

//! Multiplies a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector4 */
inline Vector4 operator*(const Vector4& vec, float a) { return Vector4(vec.coordinate.x * a, vec.coordinate.y * a, vec.coordinate.z * a, vec.coordinate.w * a); }

//! Multiplies a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector4 */
inline Vector4 operator*(float a, const Vector4& vec) { return Vector4(vec.coordinate.x * a, vec.coordinate.y * a, vec.coordinate.z * a, vec.coordinate.w * a); }

//! Multiplies a vector with a float-pointing variable and replaces the vector
/*! \related Vector4 */
inline Vector4& operator*=(Vector4& vec, float a) { vec.coordinate.x *= a; vec.coordinate.y *= a; vec.coordinate.z *= a, vec.coordinate.w *= a; return vec; }

//! Divides a vector with a float-pointing variable and returns the resulting vector
/*! \related Vector4 */
inline Vector4 operator/(const Vector4& vec, float a) { return Vector4(vec.coordinate.x / a, vec.coordinate.y / a, vec.coordinate.z / a, vec.coordinate.w / a); }

//! Divides a vector with a float-pointing variable and replaces the vector
/*! \related Vector4 */
inline Vector4& operator/=(Vector4& vec, float a) { vec.coordinate.x /= a; vec.coordinate.y /= a; vec.coordinate.z /= a; vec.coordinate.w /= a; return vec; }

/*! \defgroup sse_ext SIMD Convenience functions
    @{
*/

#ifdef HAS_SSE
//! Selects only the first value of p for the four floating-point values
#define SHUFFLE_0(p) _mm_shuffle_ps(p, p, _MM_SHUFFLE(0, 0, 0, 0))
#elif defined(HAS_ARM_NEON)
#define SHUFFLE_0(p) p[0]
#endif

#ifdef HAS_SSE
//! Selects only the second value of p for the four floating-point values
#define SHUFFLE_1(p) _mm_shuffle_ps(p, p, _MM_SHUFFLE(1, 1, 1, 1))
#elif defined(HAS_ARM_NEON)
#define SHUFFLE_1(p) p[1]
#endif

#ifdef HAS_SSE
//! Selects only the third value of p for the four floating-point values
#define SHUFFLE_2(p) _mm_shuffle_ps(p, p, _MM_SHUFFLE(2, 2, 2, 2))
#elif defined(HAS_ARM_NEON)
#define SHUFFLE_2(p) p[2]
#endif

#ifdef HAS_SSE
//! Selects only the fourth value of p for the four floating-point values
#define SHUFFLE_3(p) _mm_shuffle_ps(p, p, _MM_SHUFFLE(3, 3, 3, 3))
#elif defined(HAS_ARM_NEON)
#define SHUFFLE_3(p) p[3]
#endif

union SIMD128_ALIGNED simd128
{
#ifdef HAS_SSE
    typedef __m128 simd_type;
    simd_type   m128;
#elif defined(HAS_ARM_NEON)
    typedef float32x4_t simd_type;
    simd_type   m128;
#endif
    float       m128_f32[4];

    simd128() {}
    
    simd128(float f0, float f1, float f2, float f3)
#ifdef HAS_SSE
        :   m128(_mm_setr_ps(f0, f1, f2, f3)) {}
#else
        { m128_f32[0] = f0; m128_f32[1] = f1; m128_f32[2] = f2; m128_f32[3] = f3; }
#endif

#if defined(HAS_SSE) || defined(HAS_ARM_NEON)
    simd128(const simd_type& m)
        :   m128(m) {}

    simd128& operator=(const simd_type& m) { m128 = m; return *this; }

    inline operator simd_type() const { return m128; }
#endif
    
    float& operator[](size_t idx) { return m128_f32[idx]; }
    float operator[](size_t idx) const { return m128_f32[idx]; }
};

#ifdef HAS_SSE
//! Multiplies the four floating-point values by a single floating-point value. (SSE composite)
inline simd128 operator*(const simd128& lhs, float rhs) { return _mm_mul_ps(lhs, _mm_set_ps1(rhs)); }
#elif defined(HAS_ARM_NEON)
inline simd128 operator*(const simd128& lhs, float rhs) { return vmulq_n_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Multiplies the four floating-point values of lhs and rhs
inline simd128 operator*(const simd128& lhs, const simd128& rhs) { return _mm_mul_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128 operator*(const simd128& lhs, const simd128& rhs) { return vmulq_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Sums the four floating-point values of lhs and rhs
inline simd128 operator+(const simd128& lhs, const simd128& rhs) { return _mm_add_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128 operator+(const simd128& lhs, const simd128& rhs) { return vaddq_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Subtracts the four floating-point values of lhs and rhs
inline simd128 operator-(const simd128& lhs, const simd128& rhs) { return _mm_sub_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128 operator-(const simd128& lhs, const simd128& rhs) { return vsubq_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Divides the four floating-point values of lhs and rhs. (ARM Neon composite)
inline simd128 operator/(const simd128& lhs, const simd128& rhs) { return _mm_div_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128 operator/(const simd128& lhs, const simd128& rhs) { return vmulq_f32(lhs, vrecpeq_f32(rhs)); }
#endif

#ifdef HAS_SSE
//! Multiplies in-place the four floating-point values by a single floating-point value. (SSE composite)
inline simd128& operator*=(simd128& lhs, float rhs) { return lhs = _mm_mul_ps(lhs, _mm_set_ps1(rhs)); }
#elif defined(HAS_ARM_NEON)
inline simd128& operator*=(simd128& lhs, float rhs) { return lhs = vmulq_n_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Multiplies in-place the four floating-point values of lhs and rhs
inline simd128& operator*=(simd128& lhs, const simd128& rhs) { return lhs = _mm_mul_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128& operator*=(simd128& lhs, const simd128& rhs) { return lhs = vmulq_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Sums in-place the four floating-point values of lhs and rhs
inline simd128& operator+=(simd128& lhs, const simd128& rhs) { return lhs = _mm_add_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128& operator+=(simd128& lhs, const simd128& rhs) { return lhs = vaddq_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Subtracts in-place the four floating-point values of lhs and rhs. (ARM Neon composite)
inline simd128& operator-=(simd128& lhs, const simd128& rhs) { return lhs = _mm_sub_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128& operator-=(simd128& lhs, const simd128& rhs) { return lhs = vsubq_f32(lhs, rhs); }
#endif

#ifdef HAS_SSE
//! Divides in-place the four floating-point values of lhs and rhs. (ARM Neon composite)
inline simd128& operator/=(simd128& lhs, const simd128& rhs) { return lhs = _mm_div_ps(lhs, rhs); }
#elif defined(HAS_ARM_NEON)
inline simd128& operator/=(simd128& lhs, const simd128& rhs) { return lhs = vmulq_f32(lhs, vrecpeq_f32(rhs)); }
#endif

#ifdef HAS_SSE
inline void i_mad(simd128& res, const simd128& op_a, const simd128& op_b) { res += op_a * op_b; }
#elif defined(HAS_ARM_NEON)
inline void i_mad(simd128& res, const simd128& op_a, const simd128& op_b) { vmlaq_f32(res, op_a, op_b); }
#endif

/*! @} */

//! 4x4 homogeneous column major matrix
/*!
    \ingroup CarinaMath
*/
class SIMD128_ALIGNED Matrix4
{
    simd128 matrix[4];      /*!< the matrix data as a SIMD array */
public:
    //! Default constructor
    Matrix4() { STATIC_ASSERT(sizeof(Matrix4) == 16*sizeof(float), "Matrix4 has the wrong size"); }

    //! Constructor
    Matrix4(float* _mat);

    //! Constructor
    /*!
        \warning The constructor treats the data as column-major.
    */
    Matrix4(float m00, float m01, float m02, float m03,
            float m10, float m11, float m12, float m13,
            float m20, float m21, float m22, float m23,
            float m30, float m31, float m32, float m33);

    //! Conversion constructor
    /*! Sets the value of the matrix to the linear transformation
        represented by the rotation quaternion and the translation
        vector
        \param rotation a rotation quaternion
        \param translation a translation vector
    */
    Matrix4(const Quaternion& rotation, const Vector3& translation);
    
    //! Conversion constructor
    explicit Matrix4(const DualQuaternion& dq);

    /*! Sets the value of the matrix to the linear transformation
        represented by the rotation quaternion and the translation
        vector
    */
    void set(const Quaternion& rotation, const Vector3& translation);

    //! Assignment operator
    Matrix4& operator=(const Matrix4& mat);

    //! Multiplies this matrix by another (in-place)
    Matrix4& operator*=(const Matrix4& mat);

    //! Sums component wise this matrix with another (in-place)
    Matrix4& operator+=(const Matrix4& mat);

    //! Subtracts component wise this matrix with another (in-place)
    Matrix4& operator-=(const Matrix4& mat);

    Matrix4& operator/=(float f);
    
    //! Compares this matrix with another
    bool operator==(const Matrix4& mat) const;

    //! Gets the \f$a_{ji}\f$ element of the matrix
    float& get(unsigned i, unsigned j) { return matrix->m128_f32[i*4+j]; }

    //! Gets the \f$a_{ji}\f$ element of the matrix
    float get(unsigned i, unsigned j) const { return matrix->m128_f32[i*4+j]; }

    //! Gets the \f$a_{ji}\f$ element of the matrix
    float& operator()(unsigned i, unsigned j) { return matrix->m128_f32[i*4+j]; }

    //! Gets the \f$a_{ji}\f$ element of the matrix
    float operator()(unsigned i, unsigned j) const { return matrix->m128_f32[i*4+j]; }

    //! Get the \f$a_i\f$ column of the matrix
    float* operator[](unsigned i)
    {
    #ifndef CARINA_UNSAFE
        if(i >= 4)
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return matrix[i].m128_f32;
    }

    //! Get the \f$a_i\f$ column of the matrix
    const float* operator[](unsigned i) const
    {
    #ifndef CARINA_UNSAFE
        if(i >= 4)
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return matrix[i].m128_f32;
    }

    //! Subtracts component wise this matrix with another
    Matrix4 operator-(const Matrix4& mat) const;

    //! Sums component wise this matrix with another
    Matrix4 operator+(const Matrix4& mat) const;

    Matrix4 operator/(float f) const;
    
    //! Multiplies this matrix with another
    Matrix4 operator*(const Matrix4& mat2) const;

    //! Transforms a 3-dimensional vector
    Vector3 operator*(const Vector3& vec) const;

    //! Transforms a 4-dimensional vector
    Vector4 operator*(const Vector4& vec) const;

    //! Returns the transposed matrix
    Matrix4 transpose() const;

    //! Transposes this matrix
    void transposeSelf();

    //! Calculates the inverse matrix
    Matrix4 inverse() const;

    //! Inverts this matrix
    void invertSelf();

    //! Sets this matrix to identity matrix
    void identity();

    //! Scales the coordinate system
    /*!
        \param vec a 3-dimensional vector representing the scaling
    */
    void scale(const Vector3& vec);
    
    //! Scales the coordinate system
    /*!
        \param s scaling factor
    */
    void scale(float s);
    
    //! Rotates the coordinate system around the relative x-axis
    /*!
        \param pitch rotation around the x-axis
    */
    void rotateX(float pitch);
    
    //! Rotates the coordinate system around the relative y-axis
    /*!
        \param yaw rotation around the y-axis
    */
    void rotateY(float yaw);

    //! Rotates the coordinate system around the relative z-axis
    /*!
        \param roll rotation around the z-axis
    */
    void rotateZ(float roll);

    //! Rotates the coordinate system
    /*!
        \param angle the angle of rotation
        \param axis the relative axis of rotation
    */
    void rotate(float angle, const Vector3& axis);

    //! Rotates the coordinate system
    /*!
        \param euler a 3-dimensional vector representing the euler angles
    */
    void rotate(const Vector3& euler);

    //! Translates the coordinate system
    /*!
        \param vec a 3-dimensional vector representing the relative translation
    */
    void translate(const Vector3& vec);
    
    //! Translates the coordinate system
    /*!
        \param vec a 2-dimensional vector representing the relative translation
    */
    void translate(const Vector2& vec);

    //! Translates the coordinate system by the relative x-axis
    /*!
        \param x a floating-point variable representing the relative translation by the x-axis
    */
    void translateX(float x);

    //! Translates the coordinate system by the relative y-axis
    /*!
        \param y a floating-point variable representing the relative translation by the y-axis
    */
    void translateY(float y);

    //! Translates the coordinate system by the relative z-axis
    /*!
        \param z a floating-point variable representing the relative translation by the z-axis
    */
    void translateZ(float z);

    //! Rotates a 3-dimensional vector
    Vector3 transform_rotate(const Vector3& v) const;

    Vector3 relativeX() const;
    Vector3 relativeY() const;
    Vector3 relativeZ() const;

    Vector3 translation() const;
    Vector3 scaling() const;
    void decompose(Vector3& translation, Vector3& scaling, Vector3& euler);
};

//! Quaternion representation of spatial rotation
/*! \ingroup CarinaMath
    Quaternions are used for representing spatial rotation. In the case of dual quaternions
    and some other mathematical structures they could be used to represent other types of
    linear transfromations, e.g. translation.
*/
struct Quaternion
{
    struct coord
    {
        float x, /*!< x-coordinate component */
              y, /*!< y-coordinate component */
              z, /*!< z-coordinate component */
              w; /*!< w-coordinate component */
    };
    struct comp
    {
        Vector3 vector; /*!< the vector part of the quaternion */
        float scalar;   /*!< the scalar part of the quaternion */
    };
    union
    {
        coord coordinate;
#ifdef _CXX11_SUPPORT
        comp component;
#endif
        float elem[4]; /*!< coordinate array */
    };

    //! Default constructor
    Quaternion() {}

    //! Constructor
    /*!
        \param _x x-coordinate component
        \param _y y-coordinate component
        \param _z z-coordinate component
        \param _w w-coordinate component
    */
    Quaternion(float _x, float _y, float _z, float _w)
    {
        coordinate.x = _x;
        coordinate.y = _y;
        coordinate.z = _z;
        coordinate.w = _w;
    }

    //! Conversion constructor
    /*! Converts the rotation part of a 4x4 homogeneous matrix to quaternion */
    explicit Quaternion(const Matrix4& matrix);

    //! Copy constructor
    Quaternion(const Quaternion& quat);

    //! Assignment operator
    Quaternion& operator=(const Quaternion& quat);

    inline float& x() { return coordinate.x; }
    inline float& y() { return coordinate.y; }
    inline float& z() { return coordinate.z; }
    inline float& w() { return coordinate.w; }
#ifdef _CXX11_SUPPORT
    inline Vector3& vector() { return component.vector; }
    inline float& scalar() { return component.scalar; }
#else
    inline Vector3& vector() { return reinterpret_cast<comp*>(this)->vector; }
    inline float& scalar() { return reinterpret_cast<comp*>(this)->scalar; }
#endif

    inline float x() const { return coordinate.x; }
    inline float y() const { return coordinate.y; }
    inline float z() const { return coordinate.z; }
    inline float w() const { return coordinate.w; }
#ifdef _CXX11_SUPPORT
    inline Vector3 vector() const { return component.vector; }
    inline float scalar() const { return component.scalar; }
#else
    inline Vector3 vector() const { return reinterpret_cast<const comp*>(this)->vector; }
    inline float scalar() const { return reinterpret_cast<const comp*>(this)->scalar; }
#endif

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float& operator[](size_t i)
    {
    #ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return elem[i];
    }

    //! Array style coordinate component referencing
    /*!
        \param i the coordex of the coordinate component
        \return reference to a coordinate component
    */
    inline float operator[](size_t i) const
    {
    #ifndef CARINA_UNSAFE
        if(i >= CE_FIXED_ARRAY_SIZE(elem))
            THROW_EXCEPTION("Bad element coordex");
    #endif
        return elem[i];
    }

    //! Sets the quaternion to the multiplication identity quaternion, i.e. q = 1
    void identity();

    //! Returns the conjugate quaternion
    Quaternion conjugate() const;

    //! Conjugates this quaternion
    void conjugateSelf();

    //! Returns the inverse quaternion
    /*!
        \remarks It is the same as returning the conjugate
    */
    Quaternion inverse() const;
    
    //! Inverts the quaternion
    /*!
        \remarks It is the same as conjugating the quaternion
    */
    void invertSelf();

    //! Computes the length of the quaternion as a 4D vector
    float length() const;

    //! Normalizes the quaternion
    void normalize();

    //! Computes the dot product between two quaternion
    float dot(const Quaternion& quat) const;

    //! Rotates the coordinate system around the relative x-axis
    /*!
        \param yaw rotation around the x-axis
    */
    void rotateX(float pitch);

    //! Rotates the coordinate system around the relative y-axis
    /*!
        \param yaw rotation around the y-axis
    */
    void rotateY(float yaw);

    //! Rotates the coordinate system around the relative z-axis
    /*!
        \param yaw rotation around the z-axis
    */
    void rotateZ(float roll);

    //! Rotates the coordinate system
    /*!
        \param angle the angle of rotation
        \param axis the relative axis of rotation
    */
    void rotate(float angle, const Vector3& axis);

    //! Transforms a 3-dimensional vector
    Vector3 transform(const Vector3& v) const;
};

//! Multiplies two quaternions and stores the result in the first quaternion
/*! \related Quaternion */
Quaternion& operator*=(Quaternion& lhs, const Quaternion& rhs);

//! Sums two quaternions and stores the result in the first quaternion
/*! \related Quaternion */
Quaternion& operator+=(Quaternion& lhs, const Quaternion& rhs);

//! Subtracts two quaternions and stores the result in the first quaternion
/*! \related Quaternion */
Quaternion& operator-=(Quaternion& lhs, const Quaternion& rhs);

//! Divides two quaternions and stores the result in the first quaternion
/*! \related Quaternion */
Quaternion& operator/=(Quaternion& lhs, const Quaternion& rhs);

//! Multiplies a quaternion with a floating-point variable and stores the result in the quaternion
/*! \related Quaternion */
Quaternion& operator*=(Quaternion& quat, float f);

//! Divides a quaternion with a floating-point variable and stores the result in the quaternion
/*! \related Quaternion */
Quaternion& operator/=(Quaternion& quat, float f);

//! Compares two quaternions
/*! \related Quaternion */
bool operator==(const Quaternion& lhs, const Quaternion& rhs);

//! Divides two quaternions and returns the result
/*! \related Quaternion */
Quaternion operator/(const Quaternion& lhs, const Quaternion& rhs);

//! Multiplies two quaternions and returns the result
/*! \related Quaternion */
Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs);

//! Sums two quaternions and returns the result
/*! \related Quaternion */
Quaternion operator+(const Quaternion& lhs, const Quaternion& rhs);

//! Subtracts two quaternions and returns the result
/*! \related Quaternion */
Quaternion operator-(const Quaternion& lhs, const Quaternion& rhs);

//! Multiplies a quaternion with a floating-point variable and returns the result
/*! \related Quaternion */
Quaternion operator*(float f, const Quaternion& quat);

//! Multiplies a quaternion with a floating-point variable and returns the result
/*! \related Quaternion */
Quaternion operator*(const Quaternion& quat, float f);

//! Divides a quaternion with a floating-point variable and returns the result
/*! \related Quaternion */
Quaternion operator/(const Quaternion& quat, float f);

//! Negates a quaternion
/*! \related Quaternion */
Quaternion operator-(const Quaternion& quat);

//! Dual quaternion representation of spatial translation and rotation
/*! \ingroup CarinaMath
*/
struct DualQuaternion
{
    Quaternion  non_dual, /*!< the non-dual part */
                dual;     /*!< the dual part */

    //! Default constructor
    DualQuaternion();

    //! Constructor
    /*!
        \param ndp the non-dual part
        \param dp the dual part
    */
    DualQuaternion(const Quaternion& ndp, const Quaternion& dp);

    //! Copy constructor
    DualQuaternion(const DualQuaternion& d);

    //! Assignment operator
    DualQuaternion& operator=(const DualQuaternion& d);

    //! Conversion constructor
    /*! Convert the rotation and translation part of a 4x4 homogeneous matrix to dual quaternion
        \param mat a 4x4 homogeneous matrix
    */
    explicit DualQuaternion(const Matrix4& mat);

    //! Sets the dual quaternion to identity, i.e. q = 1
    void identity();

    //! Returns the dual conjugate dual quaternion
    DualQuaternion conjugate() const;
    
    //! Dual conjugates the dual quaternion
    void conjugateSelf();

    //! Returns the quaternion conjugate dual quaternion
    DualQuaternion conjugateQuaternion() const;

    //! Quaternion conjugates the quaternion
    void conjugateQuaternionSelf();

    //! Returns the inverse dual quaternion
    /*!
        \remarks It is the same as returning the quaternion conjugate
    */
    DualQuaternion inverse() const;

    //! Inverts the dual quaternion
    /*!
        \remarks It is the same as quaternion conjugating the dual quaternion
    */
    void invertSelf();

    //! Returns the length of the non-dual part
    float length() const;

    //! Normalizes the dual quaternion
    void normalize();

    //! Rotates the coordinate system around the relative x-axis
    /*!
        \param yaw rotation around the x-axis
    */
    void rotateX(float pitch);

    //! Rotates the coordinate system around the relative y-axis
    /*!
        \param yaw rotation around the y-axis
    */
    void rotateY(float yaw);

    //! Rotates the coordinate system around the relative z-axis
    /*!
        \param yaw rotation around the z-axis
    */
    void rotateZ(float roll);

    //! Rotates the coordinate system
    /*!
        \param angle the angle of rotation
        \param axis the relative axis of rotation
    */
    void rotate(float angle, const Vector3& axis);

    //! Translates the coordinate system
    /*!
        \param vec a 3-dimensional vector representing the relative translation
    */
    void translate(const Vector3& vec);

    //! Translates the coordinate system by the relative x-axis
    /*!
        \param x a floating-point variable representing the relative translation by the x-axis
    */
    void translateX(float x);

    //! Translates the coordinate system by the relative y-axis
    /*!
        \param y a floating-point variable representing the relative translation by the y-axis
    */
    void translateY(float y);

    //! Translates the coordinate system by the relative z-axis
    /*!
        \param z a floating-point variable representing the relative translation by the z-axis
    */
    void translateZ(float z);

    //! Transforms a 3-dimensional vector
    Vector3 transform(const Vector3& v) const;
};

//! Multiplies two dual quaternions and stores the result in the first dual quaternion
/*! \related DualQuaternion */
DualQuaternion& operator*=(DualQuaternion& lhs, const DualQuaternion& rhs);

//! Sums component-wise two dual quaternions and stores the result in the first dual quaternion
/*! \related DualQuaternion */
DualQuaternion& operator+=(DualQuaternion& lhs, const DualQuaternion& rhs);

//! Subtracts component-wise two dual quaternions and stores the result in the first dual quaternion
/*! \related DualQuaternion */
DualQuaternion& operator-=(DualQuaternion& lhs, const DualQuaternion& rhs);

//! Multiplies a dual quaternion with a floating-point variable and stores the result in the dual quaternion
/*! \related DualQuaternion */
DualQuaternion& operator*=(DualQuaternion& d, float f);

//! Compares two dual quaternions
/*! \related DualQuaternion */
bool operator==(const DualQuaternion& lhs, const DualQuaternion& rhs);

//! Multiplies two dual quaternions and returns the result
/*! \related DualQuaternion */
DualQuaternion operator*(const DualQuaternion& lhs, const DualQuaternion& rhs);

//! Sums component-wise two dual quaternion and returns the result
/*! \related DualQuaternion */
DualQuaternion operator+(const DualQuaternion& lhs, const DualQuaternion& rhs);

//! Subtracts component-wise two dual quaternion and returns the result
/*! \related DualQuaternion */
DualQuaternion operator-(const DualQuaternion& lhs, const DualQuaternion& rhs);

//! Multiplies a dual quaternion with a floating-point variable and returns the result
/*! \related DualQuaternion */
DualQuaternion operator*(const DualQuaternion& d, float f);

//! Multiplies a dual quaternion with a floating-point variable and returns the result
/*! \related DualQuaternion */
DualQuaternion operator*(float f, const DualQuaternion& d);

//! Negates component-wise a dual quaternion and returns the result
/*! \related DualQuaternion */
DualQuaternion operator-(const DualQuaternion& q);

//! Calculates the linear interpolation between two dual quaternion
/*! This function uses the following equation: \f$(1-t)*q_1 + t*q_2\f$
    \param q1 a dual quaternion
    \param q1 a dual quaternion
    \param t the distance between the two quaternions (\f$t \in [0, 1]\f$)
    \related DualQuaternion 
*/
DualQuaternion interpolate(float t, const DualQuaternion& q1, const DualQuaternion& q2);

struct Ray
{
    Vector3 begin,
            direction;

    Ray(const Vector3& b, const Vector3& d)
        :   begin(b),
            direction(d) {}
};

inline Ray operator*(const Matrix4& mat, const Ray& r)
{
    return Ray(mat*r.begin, mat.transform_rotate(r.direction));
}

struct Segment
{
    Vector3 begin,
            end;

    Segment(const Vector3& b, const Vector3& e)
        :   begin(b),
            end(e) {}

    Vector3 getMidPoint() const { return (end - begin)*0.5f; }
};

inline Segment operator*(const Matrix4& mat, const Segment& seg)
{
    return Segment(mat*seg.begin, mat*seg.end);
}

//! Inserts a 4-dimesional vector into an output stream
/*! \related Vector4 */
std::ostream& operator<<(std::ostream& o, const Vector4& vec);

//! Inserts a 3-dimesional vector into an output stream
/*! \related Vector3 */
std::ostream& operator<<(std::ostream& o, const Vector3& vec);

//! Inserts a 2-dimesional vector into an output stream
/*! \related Vector2 */
std::ostream& operator<<(std::ostream& o, const Vector2& vec);

//! Inserts a 4x4 homogeneous matrix into an output stream
/*! \related Matrix4 */
std::ostream& operator<<(std::ostream& o, const Matrix4& mat);

//! Inserts a quaternion into an output stream
/*! \related Quaternion */
std::ostream& operator<<(std::ostream& o, const Quaternion& quat);

//! Inserts a dual quaternion into an output stream
/*! \related DualQuaternion */
std::ostream& operator<<(std::ostream& o, const DualQuaternion& dq);

//! Extracts a 4-dimensional vector from an input stream
/*! \related Vector4 */
std::istream& operator>>(std::istream& i, Vector4& vec);

//! Extracts a 3-dimensional vector from an input stream
/*! \related Vector3 */
std::istream& operator>>(std::istream& i, Vector3& vec);

//! Extracts a 2-dimensional vector from an input stream
/*! \related Vector2 */
std::istream& operator>>(std::istream& i, Vector2& vec);

//! Extracts a quaternion from an input stream
/*! \related Quaternion */
std::istream& operator>>(std::istream& i, Quaternion& quat);

//! Extracts a dual quaternion from an input stream
/*! \related DualQuaternion */
std::istream& operator>>(std::istream& i, DualQuaternion& dq);

//! Extracts a 4x4 homogeneous matrix from an input stream
/*!
    \remarks The matrix is entered as row-major
    \related Matrix4 
*/
std::istream& operator>>(std::istream& i, Matrix4& mat);
}

#endif /* MATH_HH_ */
